#include "stdafx.h"
#include "PointMass.h"

using namespace ani;

PointMass::PointMass()
{
	mForceAccum = glm::vec3(0);
	mVelocity = glm::vec3(0);

	mPrevPosition = mPosition;
	mFirstIter = true;
}

PointMass::PointMass(const glm::vec3 & position, float mass)
{
	assert(mass != 0.0f);

	mPosition = position;
	mMass = mass;
	mMassInverse = 1.0f / mass;

	mForceAccum = glm::vec3(0);
	mVelocity = glm::vec3(0);

	mPrevPosition = mPosition;
	mFirstIter = true;
}

void PointMass::euler(float dt)
{
	glm::vec3 accel = mForceAccum * mMassInverse; // <=> mForceAccum / mMass
	mVelocity += dt * accel;
	mPosition += dt * mVelocity;
}

void PointMass::verlet(float dt)
{
	if(mFirstIter)
	{
		mPrevPosition = mPosition;
		mPosition += dt * mVelocity;

		mFirstIter = false;
	}
	else
	{
		glm::vec3 accel = mForceAccum * mMassInverse; // <=> mForceAccum / mMass
		glm::vec3 newPosition = mPosition * 2.0f - mPrevPosition + dt * dt * accel;
		mPrevPosition = mPosition;
		mPosition = newPosition;
	}

	mVelocity = (mPosition - mPrevPosition) / dt;
}

void PointMass::integrate(float dt)
{
	euler(dt);
	//verlet(dt);

	mForceAccum = glm::vec3(0);
}

void PointMass::addForce(const glm::vec3 & force)
{
	mForceAccum += force;
}